#include "linkManager.h"
#include <Eigen/SVD>
#include <time.h>
#define EPSILON 0.0001
#define TOLERANCE 0.05
LinkManager::LinkManager(){
	links = vector<Link*>();
}
void LinkManager::initLink(Vector3f origin, float t, float p,  float l, float glb_t, float glb_p){
	Link* ini = new Link(origin, t, p, l, glb_t, glb_p);
	links.push_back(ini);
}
void LinkManager::addLink(float t, float p,  float l, float glb_t, float glb_p){
	float temp_theta = 0;
	float temp_phi = 0;
	for(int i=0; i<links.size(); i++){
		temp_theta += links[i]->theta;
		temp_phi += links[i]->phi;
	}
	Link* temp = links[links.size()-1];
	Vector3f pos = Vector3f(temp->pos(0) + temp->length*sin(temp_theta)*cos(temp_phi), 
							temp->pos(1) + temp->length*sin(temp_theta)*sin(temp_phi), 
							temp->pos(2) + temp->length*cos(temp_theta));
	
	Link* new_link = new Link(pos, t, p, l, glb_t, glb_p);
	links.push_back(new_link);
}

Matrix<float, 2, 3> LinkManager::get2D3LJacobian(float theta1, float theta2, float theta3){
	float l_1 = links[0]->length;
	float l_2 = links[1]->length;
	float l_3 = links[2]->length;
	Matrix<float,2 ,3> m;
	m(0,0) = l_1*cos(theta1) + l_2*cos(theta1+theta2)+ l_3*cos(theta1+theta2+theta3);
	m(0,1) = l_2*cos(theta1+theta2) + l_3*cos(theta1+theta2+theta3);
	m(0,2) = l_3*cos(theta1+theta2+theta3);
	m(1,0) = -l_1*sin(theta1) - l_2*sin(theta1+theta2) - l_3*sin(theta1+theta2+theta3);
	m(1,1) = -l_2*sin(theta1+theta2) - l_3*sin(theta1+theta2+theta3);
	m(1,2) = -l_3*sin(theta1+theta2+theta3);
	return m;
}
Matrix<float, 3, 6> LinkManager::get3D3LJacobian(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3){
	float l_1 = links[0]->length;
	float l_2 = links[1]->length;
	float l_3 = links[2]->length;
	Matrix<float,3,6> m;
	for (int i = 0; i < 3; i ++) {
		m(i,0) = (p(theta1 + EPSILON, theta2, theta3, phi1, phi2, phi3)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;
		m(i,1) = (p(theta1, theta2 + EPSILON, theta3, phi1, phi2, phi3)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;
		m(i,2) = (p(theta1, theta2, theta3 + EPSILON, phi1, phi2, phi3)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;	
		m(i,3) = (p(theta1, theta2, theta3, phi1 + EPSILON, phi2, phi3)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;
		m(i,4) = (p(theta1, theta2, theta3, phi1, phi2 + EPSILON, phi3)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;
		m(i,5) = (p(theta1, theta2, theta3, phi1, phi2, phi3 + EPSILON)[i] - p(theta1, theta2, theta3, phi1, phi2, phi3)[i]) / EPSILON;
	}
	return m;
}

void LinkManager::calculate(Vector3f end){
	float theta1 = links[0]->theta;
	float theta2 = links[1]->theta; 
	float theta3 = links[2]->theta; 
	float phi1 = links[0]->phi;
	float phi2 = links[1]->phi;
	float phi3 = links[2]->phi;
	
	srand(time(NULL));

	Matrix<float, 6, 1> psi;
	psi(0,0) = theta1; 
	psi(1,0) = theta2;
	psi(2,0) = theta3;
	psi(3,0) = phi1;
	psi(4,0) = phi2;
	psi(5,0) = phi3;
	Matrix<float, 6, 1> psiStart;
	psiStart(0,0) = theta1; 
	psiStart(1,0) = theta2;
	psiStart(2,0) = theta3;
	psiStart(3,0) = phi1;
	psiStart(4,0) = phi2;
	psiStart(5,0) = phi3;
	Vector3f origin = links[0]->pos;
	float len = links[0]->length + links[1]->length + links[2]->length - EPSILON;
	if((end-origin).norm() > len){
		end = origin + (end - origin) * (len / (end-origin).norm()); 
	}
	while((pi(psi, end)).norm()>=TOLERANCE) {
		MatrixXf m = MatrixXf::Random(3,6);
		Matrix<float, 3, 6> j = get3D3LJacobian(theta1, theta2, theta3, phi1, phi2, phi3);
		JacobiSVD<MatrixXf> svd(m);
		svd.compute(j, ComputeFullU|ComputeFullV);
	    Matrix3f u = svd.matrixU();
		Matrix<float, 6, 6> v = svd.matrixV();
		Vector3f s = svd.singularValues();
	
		Matrix3f u_trans = u.transpose();

		Matrix<float, 6, 3> s_inverse;
		s_inverse.setZero(6, 3);
		for (int i = 0; i < 3; i++) {
			if(fabs(s(i)) > EPSILON)
				s_inverse(i, i) = 1/s(i);
		}
		
		Matrix<float, 6, 3> T = v * s_inverse * u_trans;

		psi = psi - T*pi(psi, end);  
		theta1 = psi(0);
		theta2 = psi(1);
		theta3 = psi(2);
		phi1 = psi(3);
		phi2 = psi(4);
		phi3 = psi(5);
		
	}

	
	links[0]->theta = theta1;
    links[0]->global_theta = theta1;
	links[1]->theta = theta2; 
    links[1]->global_theta = theta1 + theta2;
	links[2]->theta = theta3; 
    links[2]->global_theta = theta1 + theta2 + theta3;
	
	links[0]->phi = phi1;
    links[0]->global_phi = phi1;
	links[1]->phi = phi2;
    links[1]->global_phi = phi1 + phi2;
	links[2]->phi = phi3;
    links[2]->global_phi = phi1 + phi2 + phi2;

	Vector3f p1 = Vector3f(links[0]->pos(0)+ links[0]->length*sin(theta1)*cos(phi1),
						   links[0]->pos(1)+ links[0]->length*sin(theta1)*sin(phi1),
						   links[0]->pos(2)+ links[0]->length*cos(theta1));
	links[1]->pos = p1;

	Vector3f p2 = Vector3f(links[1]->pos(0)+ links[1]->length*sin(theta1+theta2)*cos(phi1+phi2),
						   links[1]->pos(1)+ links[1]->length*sin(theta1+theta2)*sin(phi1+phi2),
						   links[1]->pos(2)+ links[1]->length*cos(theta1+theta2));

	links[2]->pos = p2;




	for(int i=0; i<3 ; i++){
		Vector3f endPos = links[i]->getEnd();

		for(int j=0; j< links[i]->vertices.size(); j++){
            float length = links[i]->vertices[j]->length;
            float theta = links[i]->vertices[j]->theta;
            float g_theta = links[i]->global_theta; // links[i]'s global theta
            float phi = links[i]->vertices[j]->phi;
            float g_phi = links[i]->global_phi; // links[i]'s global phi
            float x = endPos[0] + length*sin(theta + g_theta)*cos(phi + g_phi);
            float y = endPos[1] + length*sin(theta + g_theta)*sin(phi + g_phi);
            float z = endPos[2] + length*cos(theta + g_theta);
            links[i]->vertices[j]->myVertex = Vector3f(x,y,z);
		}
	}

}

Vector3f LinkManager::pi(Matrix<float, 6, 1> theta, Vector3f g){
	return p(theta[0], theta[1], theta[2], theta[3], theta[4], theta[5]) - g;
}

Vector3f LinkManager::getEndPos(){
	return p(links[0]->theta, links[1]->theta, links[2]->theta, links[0]->phi, links[1]->phi, links[2]->phi);
}
Vector3f LinkManager:: p(float theta1, float theta2, float theta3, float phi1, float phi2, float phi3) {
	float l1 = links[0]->length;
	float l2 = links[1]->length;
	float l3 = links[2]->length;
	Vector3f origin = links[0]->pos;

	float p1x = origin[0] + l1 * sin(theta1)*cos(phi1);
	float p1y = origin[1] + l1 * sin(theta1)*sin(phi1);
	float p1z = origin[2] + l1 * cos(theta1);

	float p2x = p1x + l2 * sin(theta1 + theta2) * cos(phi1 + phi2);
	float p2y = p1y + l2 * sin(theta1 + theta2) * sin(phi1 + phi2);
	float p2z = p1z + l2 * cos(theta1 + theta2);

	float p3x = p2x + l3 * sin(theta1 + theta2 + theta3) * cos(phi1 + phi2 + phi3);
	float p3y = p2y + l3 * sin(theta1 + theta2 + theta3) * sin(phi1 + phi2 + phi3);
	float p3z = p2z + l3 * cos(theta1 + theta2 + theta3);
	Vector3f vec(p3x,p3y,p3z);
	return vec;
}

void LinkManager::display(){
	float theta1 = links[0]->theta;
	float theta2 = links[1]->theta;
	float theta3 = links[2]->theta;
	float phi1 = links[0]->phi;
	float phi2 = links[1]->phi;
	float phi3 = links[2]->phi;
	
	draw(links[0]->pos, theta1, phi1, links[0]->length);
	
	draw(links[1]->pos, theta1+theta2,  phi1+phi2, links[1]->length);
	
	draw(links[2]->pos, theta1+theta2+theta3, phi1 + phi2 + phi3, links[2]->length);
}

void LinkManager::drawCube(float x , float y , float z){
	 glBegin(GL_QUADS);		// Draw The Cube Using quads
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Top)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Top)

    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Top Right Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Top Left Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Bottom)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Bottom)

    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Front)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Front)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Front)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Front)
 
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Top Right Of The Quad (Back)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Top Left Of The Quad (Back)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Back)
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Back)
 
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Left)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Left)
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Right)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Right)
    glEnd();			// End Drawing The Cube
}
void LinkManager::draw(Vector3f pos, float theta, float phi, float length){
	glColor3f(0,0,1);
	drawCube(pos(0), pos(1), pos(2));
	drawCube(pos(0)+ length*sin(theta)*cos(phi), pos(1)+ length*sin(theta)*sin(phi), pos(2)+ length*cos(theta));
	glColor3f(1, 1, 1);

	glBegin(GL_LINES);
	glVertex3f(pos(0), pos(1), pos(2));
	glVertex3f(pos(0)+ length*sin(theta)*cos(phi), pos(1)+length*sin(theta)*sin(phi), pos(2)+length*cos(theta));
	glEnd();
}
